var collideVecToGround = function(vec, ground, onCollision) {

    var h = ground.getHeight(vec.z, vec.x);
    var normal = ground.getNormal(vec.z, vec.x);

    if (vec.y < h) 
    	onCollision(h, normal);
};

var collidePointToGround = function(entity, point, pointIx, ground) {

    collideVecToGround(point.pos, ground, function(h, normal) {

    	if (point.vel.dot(normal) < 0) {

			entity.respondGroundHit(pointIx, normal);
			point.pos.y = h;
			point.groundContact = true;

    	}
    });
};

var collidePointToSolid = function(entity, point, pointIx, solidPhyObj) {

	solidPhyObj.testCollision(point.pos, function(normal, penetration, isHollow) {
		if (isHollow) {
			point.inHollowSolid = true;
		}
		else {
			entity.respondGroundHit(pointIx, normal);
			point.pos.addMe( normal.scale(penetration) );
			point.groundContact = true;
		}		
	});
};

var forEachEntity = function(entities, callback, startIx) {
    for (var i = startIx || 0; i < entities.length; i++)
	callback(entities[i], i);
};

var handleEntityCollisions = function(e1, e1ix, entities, ground) {
    
    var phyObj = e1.getPhyObj();
    if (phyObj.isSolid())
    	return;
    
    phyObj.eachPoint(function(point, pointix) {

	point.groundContact = false;
	point.inHollowSolid = false;
    
    	forEachEntity(entities, function(e2) {

	    var e2PhyObj = e2.getPhyObj();
	    if (!e2PhyObj.isSolid())
		return;
	    collidePointToSolid(e1, point, pointix, e2PhyObj);
        }, e1ix);

    	if (!point.inHollowSolid)
	    collidePointToGround(e1, point, pointix, ground);
	    
    });
    
    forEachEntity(entities, function(ent) {
		
	if (!ent.isPortal)
	    return;
	
	// FIXME: Clean up. Seriously.
	var sourcePortal = ent.entModel;
	
	// Magically know that the zeroth Solid is the special one!
	var portalSolid = ent.getPhyObj().solids[0];
	
	portalSolid.getPhyObj().testCollision(e1.centerVec, function(normal, penetration, isHollow) {
	    
	    var isForward = e1.getPhyObj().points[0].vel.dot(sourcePortal.getForwardNormal()) < 0;
	    
	    var destPortal = isForward ?
		sourcePortal.getForwardPortal() : sourcePortal.getBackwardPortal();
	    
	    if (destPortal) {
		
		var dstFwdVec = destPortal.getForwardNormal();
		
		if (isForward)
		    dstFwdVec = dstFwdVec.scale(-1);
		
		var displacement = destPortal.getPos().sub(sourcePortal.getPos());
		displacement.addMe(Vec(0, 2, 0)).addMe(dstFwdVec.scale(5));
		
		phyObj.eachPoint(function(p) { p.pos.addMe(displacement); });
		
	    }
	    
	}, true);
	
    } );
    
};

var handleCollisions = function(entities, ground) {
    forEachEntity(entities, function(entity, ix) {
	    handleEntityCollisions(entity, ix, entities, ground);
	});
};

var updateEntities = function(entities) {
	if (doUpdateEntities) {
		for (var i = 0; i < entities.length; i++) {
			entities[i].update();
		};
	}
};

step = function(game, dt) {
    satisfyConstraints(game.state);
    verletIntegrate(game.state, dt);
    handleCollisions(game.state.entities, game.groundMap);
    updateEntities(game.state.entities);
    processThoughts(game.state);
};

verletIntegrate = function(state, dt) {
    var entity, entities = state.entities;	
    for (var i = 0; i < entities.length; i++) {
		entity = entities[i];
		entity.setAcceleration();
		entity.phyObj.verletIntegrate(dt);
	}
};

processThoughts = function(state) {
    var entities = state.entities;
    for (var i = 0; i < entities.length; i++) {
		entities[i].think();
    }
};

satisfyConstraints = function(state) {
    var entities = state.entities;
    for (var i = 0; i < entities.length; i++)
    	entities[i].getPhyObj().satisfyConstraints();
};

Time = function() {
    this.prevT = Date.now();
    this.frameId = 0;
    this.step = 1000/60; // milliseconds (60 fps)
};
Time.prototype.hasTick = function() {
    var now = Date.now();
    return (now - this.prevT) > this.step;
};
// Return step size in seconds
Time.prototype.getTick = function() {
    this.prevT += this.step;
    this.frameId++;
    return this.step/1000;
};
Time.prototype.getFrameId = function() {
    return this.frameId;
};
Time.prototype.getFrameInfo = function() {
    return {
	frameId: this.frameId,
	start: this.prevT
    };
};
